/*============================================================================*/
/*                        Tortoise Team			                              */
/*============================================================================*/
/*                        OBJECT SPECIFICATION                                */
/*============================================================================*
* C Source:         %HAL_ServoMotor.c%
* Instance:         RPL_1
* %version:         1 %
* %created_by:      Andres Torres Garcia %
* %date_created:    Sunday May 20 13:41:01 2012 %
*=============================================================================*/
/* DESCRIPTION : C source file for the hardware layer of the servo motor      */
/*============================================================================*/
/* FUNCTION COMMENT : Contains the function definitions of the hardware		  */
/*	layer of the servo motor		                                          */
/*                                                                            */
/*============================================================================*/
/*                               OBJECT HISTORY                               */
/*============================================================================*/
/*  REVISION |   DATE      |                               |      AUTHOR      */
/*----------------------------------------------------------------------------*/
/*  1.0      | 26/05/2012 |                               | Andres Torres	  */
/* Integration under Continuus CM                                             */
/*============================================================================*/

/* Includes */
/* -------- */
#include "stdtypedef.h"
#include "MAL_PWM.h"
#include "MAL_EMIOS.h"
#include "MAL_IOAPI.h"

/* Functions macros, constants, types and datas         */
/* ---------------------------------------------------- */
/* Functions macros */

/*==================================================*/ 
/* Definition of constants                          */
/*==================================================*/ 
/* BYTE constants */

/* WORD constants */

/* LONG and STRUCTURE constants */


/*======================================================*/ 
/* Definition of RAM variables                          */
/*======================================================*/ 
/* BYTE RAM variables */

/* WORD RAM variables */

/** \brief Current value of the servo motor */
static T_UWORD ServoMotorValue;

/* LONG and STRUCTURE RAM variables */

/*======================================================*/ 
/* close variable declaration sections                  */
/*======================================================*/ 

/* Private defines */

/*! \def SERVO_CHANNEL
  Channel of the servo motor                                                                                      
 */
#define SERVO_CHANNEL			4

/*! \def MOTORS_MCB_CHANNEL		
  Channel of the counter
 */
#define MOTORS_MCB_CHANNEL		0

/*! \def SERVO_MIN_US			
  Minimum value of the servo motor in us
 */
#define SERVO_MIN_US			1020

/*! \def SERVO_MAX_US			
  Maximum value of the servo motor in us
 */													
#define SERVO_MAX_US			1520													

/*! \def SERVO_MEDIUM_US			
  Medium value of the servo motor in us
 */
#define SERVO_MEDIUM_US			((SERVO_MAX_US - SERVO_MIN_US)/2 + SERVO_MIN_US)

/*! \def SERVO_PIN				
  Pin connector to the servo motor                                                                              
 */
#define SERVO_PIN				PCR_EMIOS_0_4_1

/*! \def MOTORS_PERIOD_US		
  Period of the servo motor in us                                                                              
 */
#define MOTORS_PERIOD_US		20000

/*! \def COUNTER_BUS
  The counter bus for the PWM                                                                              
 */
#define COUNTER_BUS		1


/* Private functions prototypes */
/* ---------------------------- */


/* Exported functions prototypes */
/* ----------------------------- */
void vfnInit_ServoMotor(void);
void vfnSet_ServoMotor(T_UWORD uw_pos, T_UWORD uw_minVal, T_UWORD uw_maxVal);
T_UWORD GetServoMotor_Value(void);
/* Inline functions */
/* ---------------- */

/* Private functions */
/* ----------------- */

/* Exported functions */
/* ------------------ */

/**************************************************************
 *  Name                 :	GetServoMotor_Value
 *  Description          :	Get the current value of the servo motor
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/ 
T_UWORD GetServoMotor_Value(void)
{
	return ServoMotorValue;
}
  
/**************************************************************
 *  Name                 :	vfnInit_ServoMotor
 *  Description          :	Initialize the servo motor
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/  
void vfnInit_ServoMotor(void)
{
	IO_ChangeFctnPin(14, 1);

	vfnSetUpClock();

	vfnInitPWMChannel(SERVO_CHANNEL, SERVO_MEDIUM_US, MOTORS_MCB_CHANNEL, MOTORS_PERIOD_US, COUNTER_BUS, SERVO_PIN);
	
	//When you join this module to another modules, change the place of this instruction.
	//START_COUNTING();
}


/**************************************************************
 *  Name                 :	vfnSet_ServoMotor
 *  Description          :	Set the servo motor value on respect with the limits.
 *  Parameters           :  [Input: uw_pos = position of the servo motor; uw_minVal: Lower limit; uw_maxVal= Higher limit , Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
void vfnSet_ServoMotor(T_UWORD uw_pos, T_UWORD uw_minVal, T_UWORD uw_maxVal)
{
	if(uw_pos < uw_minVal)
	{
		uw_pos = uw_minVal;
	}
	else if(uw_pos > uw_maxVal)
	{
		uw_pos = uw_maxVal;
	}
	
	ServoMotorValue = (T_UWORD) (  ( ( (uw_pos - uw_minVal) * (SERVO_MAX_US - SERVO_MIN_US) ) / (uw_maxVal - uw_minVal) ) + SERVO_MIN_US ); 
	
	vfnSetDutyCycle(SERVO_CHANNEL, ServoMotorValue);
}
